#include "splineinterpolator.h"
#include <stdexcept>
#include <algorithm>

SplineInterpolator::SplineInterpolator()
{
    // 构造函数（目前无特殊逻辑）
}

// 样条插值函数
std::vector<double> SplineInterpolator::interpolate(const std::vector<double>& t, const std::vector<double>& y, const std::vector<double>& t2) {
    // 确保 t 是严格单调递增的
    if (!std::is_sorted(t.begin(),  t.end()))  {
        throw std::invalid_argument("Time points t must be strictly monotonically increasing.");
    }

    // 计算三次样条插值的系数
    calculateCoefficients(t, y);

    // 在新时间点 t2 上进行插值
    std::vector<double> y2;
    for (double t2_val : t2) {
        y2.push_back(evaluateSpline(t2_val));
    }

    return y2;
}

// 计算三次样条插值的系数
void SplineInterpolator::calculateCoefficients(const std::vector<double>& t, const std::vector<double>& y) {
    // 初始化时间点数据
    this->t = t;

    int n = t.size()  - 1;

    // 初始化系数
    a.resize(n);
    b.resize(n);
    c.resize(n);
    d.resize(n);

    // 计算 h 和 alpha
    std::vector<double> h(n), alpha(n);
    for (int i = 0; i < n; ++i) {
        h[i] = t[i + 1] - t[i];
        if (i > 0) {
            alpha[i] = (3.0 / h[i]) * (y[i + 1] - y[i]) - (3.0 / h[i - 1]) * (y[i] - y[i - 1]);
        }
    }

    // 计算 c 系数（三对角矩阵求解）
    std::vector<double> l(n), mu(n), z(n);
    l[0] = 1.0;
    mu[0] = 0.0;
    z[0] = 0.0;

    for (int i = 1; i < n; ++i) {
        l[i] = 2.0 * (t[i + 1] - t[i - 1]) - h[i - 1] * mu[i - 1];
        mu[i] = h[i] / l[i];
        z[i] = (alpha[i] - h[i - 1] * z[i - 1]) / l[i];
    }

    // 回代求解 c
    c[n - 1] = 0.0;
    for (int i = n - 2; i >= 0; --i) {
        c[i] = z[i] - mu[i] * c[i + 1];
    }

    // 计算 b 和 d 系数
    for (int i = 0; i < n; ++i) {
        b[i] = (y[i + 1] - y[i]) / h[i] - h[i] * (c[i + 1] + 2.0 * c[i]) / 3.0;
        d[i] = (c[i + 1] - c[i]) / (3.0 * h[i]);
    }

    // 保存 a 系数
    for (int i = 0; i < n; ++i) {
        a[i] = y[i];
    }
}

// 在插值点 t2 上计算插值结果
double SplineInterpolator::evaluateSpline(double t2) const {
    // 找到 t2 所在的区间
    int n = a.size();
    int i = 0;
    while (i < n && t2 > t[i + 1]) {
        ++i;
    }

    // 计算插值结果
    double dt = t2 - t[i];
    return a[i] + b[i] * dt + c[i] * dt * dt + d[i] * dt * dt * dt;
}
